/*
 * Copyright The Slam Authors
 */

#include "slam/mapping/internal/2d/check_loop_constraints.h"
#include "glog/logging.h"

namespace slam {
namespace mapping {

void CheckLoopConstraints::Reset() {
  node_data_bak_.clear();
  submap_data_bak_.clear();
  landmark_data_bak_.clear();
  current_trajectory_id_ = 0;
}

void CheckLoopConstraints::SetCurrentTrajectoryId(const int &trajectory_id ) {
  current_trajectory_id_ = trajectory_id;
}

void CheckLoopConstraints::BackupOptimizationData(
      const optimization::OptimizationProblem2D * const optimization_problem) {
  const auto & node_data = optimization_problem->node_data();
  const auto & submap_data = optimization_problem->submap_data();
  const auto & landmark_data = optimization_problem->landmark_data();

  for (const auto & item : node_data) {
    if (item.id.trajectory_id == current_trajectory_id_) {
      if (node_data_bak_.count(item.id) != 0) {
        node_data_bak_.at(item.id) = item.data;
      } else {
        node_data_bak_.emplace(item.id, item.data);
      }
    }
  }

  for (const auto & item : submap_data) {
    if (item.id.trajectory_id == current_trajectory_id_) {
      if (submap_data_bak_.count(item.id) != 0) {
        submap_data_bak_.at(item.id) = item.data;
      } else {
        submap_data_bak_.emplace(item.id, item.data);
      }
    }
  }

  for (const auto & item : landmark_data) {
    if (landmark_data_bak_.count(item.first) != 0) {
      landmark_data_bak_.at(item.first) = item.second;
    } else {
      landmark_data_bak_.emplace(item.first, item.second);
    }
  }
}

void CheckLoopConstraints::ResetOptimizationData(
      optimization::OptimizationProblem2D * const optimization_problem) {
  auto  node_data = optimization_problem->mutable_node_data();
  auto  submap_data = optimization_problem->mutable_submap_data();
  auto  landmark_data = optimization_problem->mutable_landmark_data();

  for (auto iter = node_data_bak_.begin(); 
      iter != node_data_bak_.end();) {
    if (node_data->Contains(iter->first)) {
      node_data->at(iter->first) = iter->second;
      ++iter;
    } else {
      iter = node_data_bak_.erase(iter);
    }
  }

  for (auto iter = submap_data_bak_.begin(); 
      iter != submap_data_bak_.end();) {
    if (submap_data->Contains(iter->first)) {
      submap_data->at(iter->first) = iter->second;
      ++iter;
    } else {
      iter = submap_data_bak_.erase(iter);
    }
  }

  for (auto iter = landmark_data_bak_.begin(); 
      iter != landmark_data_bak_.end();) {
    if (landmark_data->count(iter->first) != 0) {
      landmark_data->at(iter->first) = iter->second;
      ++iter;
    } else {
      iter = landmark_data_bak_.erase(iter);
    }
  }
}

bool CheckLoopConstraints::DeleteErrorConstraint(
    PoseGraphData * pose_graph_data,
    const optimization::OptimizationProblem2D * const optimization_problem) {
  bool is_constraint_ok = true;
  const auto & node_data = optimization_problem->node_data();
  const auto & submap_data = optimization_problem->submap_data();
  if (pose_graph_data->submap_data.size() < 3) {
    return true;
  }
  const int  num_range_data = 
      pose_graph_data->submap_data.begin()->data.submap->num_range_data()/2;

  for(auto iter = pose_graph_data->constraints.begin(); 
      iter != pose_graph_data->constraints.end();) {
    if(iter->tag == PoseGraphInterface::Constraint::INTRA_SUBMAP) {
      ++iter;
      continue;
    }

    if(std::fabs(iter->node_id.node_index - 
        (iter->submap_id.submap_index + 1) * num_range_data) < 
            submap_interval_*num_range_data) {
      ++iter;
      continue;
    }

    const transform::Rigid2d relative_pose_constraint = 
        transform::Project2D(iter->pose.zbar_ij);
    const transform::Rigid2d relative_pose_optimized = 
        submap_data.at(iter->submap_id).global_pose.inverse() *
            node_data.at(iter->node_id).global_pose_2d;
    const transform::Rigid2d difference = 
        relative_pose_constraint.inverse() * relative_pose_optimized;
    const double diff_angle =  
        common::NormalizeAngleDifference(
              transform::GetAngle(transform::Embed3D(difference)));

    if(difference.translation().norm() >  max_delta_translation_ ||
        diff_angle > max_delta_rotation_) {
      iter = pose_graph_data->constraints.erase(iter);
      is_constraint_ok = false;
      LOG(WARNING) <<"Bad constraint betwen node: "<<iter->node_id
          <<" and submap : "<<iter->submap_id<<", difference = "<<difference.translation().x()
          <<"  "<<difference.translation().y()<<"  "<<diff_angle;  
    } else {
      ++iter;    
    }
  }
  return is_constraint_ok;
}


}
}